#include <Eigen/Dense>
#include <math.h>
#include <iostream>
#include <stdio.h>

#define THRES 1
#define WHICH_PARAM 7

#define PARAM_1 20.7
#define PARAM_2 2.2
#define PARAM_3 (-183.8)

//theta, alpha, a, s
Eigen::Matrix<double, 7, 4> denavitMat;


Eigen::Matrix<double, 4, 1> vMat;

Eigen::Matrix4d getM(double theta_deg, double alpha_deg, double a, double s) {

	//std::cout << theta << " " << alpha << std::endl;
	double theta = theta_deg * M_PI / 180;
	double alpha = alpha_deg * M_PI / 180;
	//std::cout << theta << " " << alpha << std::endl;
	Eigen::Matrix4d mat;
	mat << 
		cos(theta), -sin(theta)*cos(alpha),  sin(theta)*sin(alpha), a*cos(theta),
		sin(theta),  cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta),
		         0,      		sin(alpha), 			cos(alpha),            s,
				 0,                      0,                      0,            1;
	return mat;
}

int main() {
	denavitMat <<
	0, 0, 33.5, 0,
	0, -90, 0, 0,
	90, 90, 0, 0,
	10, 180, 80, 0,
	20, 0, 80, 0,
	-10, -90, 0, 0,
	0, 0, 27.5, 0;

	vMat << 1,1,1,1;
	//std::cout << vMat << std::endl;
	//std::cout << denavitMat << std::endl;
	Eigen::Matrix4d m_i = Eigen::Matrix4d::Identity();
	Eigen::Matrix4d mulMat = m_i;
	for (int i = 0; i < denavitMat.rows(); i++) {
		double add = (i == 0)? 0:5;

		Eigen::Matrix4d newM = getM(denavitMat(i, 0) + add, denavitMat(i, 1), denavitMat(i, 2), denavitMat(i, 3));
		//mulMat = m_i * newM;
		mulMat = m_i * newM;
		Eigen::Vector4d v_i = mulMat * vMat;

		std::cout << "i = " << i << ": ";
		if (i < WHICH_PARAM || WHICH_PARAM == -1) {
			m_i = mulMat;
			
			printf("[%f, %f, %f]\n", v_i(0,0), v_i(1,0), v_i(2,0));
			continue;
		} else {
			printf("\n");
			for (double r1 = 0; r1 < 360; r1+=10) {
				for (double r2 = 0; r2 < 360; r2+=10) {
					for (double j = 0; j < 100; j+=0.5) {
						for (double k = 0; k < 100; k+=0.5) {
							newM = getM(add + r1, r2, j, k);
						
							v_i = m_i * newM * vMat;

							if (fabs(v_i(0,0) - PARAM_1) < THRES && fabs(v_i(1,0) - PARAM_2) < THRES && fabs(v_i(2,0) - PARAM_3) < THRES) {
								printf("[%.3f, %.3f, %.3f] with (%.2f, %.2f, %.2f, %.2f)\n", v_i(0,0), v_i(1,0), v_i(2,0), r1, r2, j, k);
								// std::cout << " vals were " << r1 << " " << r2 << " " << j << " " << k << std::endl;	
								
							}
						}	
					}
				}
			}	
		}

		
	}
	return 0;
}